using System;
using Microsoft.SPOT;
using System.Text;
using System.Threading;

namespace FEZ_Cobra_Console_Application1
{
    public class Commander
    {
        double r; 
        Motors m;
        private int speed = 0;
        public Thread bkgrTh;
        public bool isCheckingAhead = true;     //change this to false to disable semi automous feature

        public Commander()
        {
            m = Motors.getMotor();
            r = Readouts.getIRReading();

            bkgrTh = new Thread(new ThreadStart(checker));
            //bkgrTh.Start();
        }

        /// <summary>
        /// When a new command is read, this is the service loop that is executed every time. It uses a sequence of switch statements for efficiency
        /// </summary>
        /// <param name="s"></param>
        public void processCommands(string s)
        {
            char[] dsa = s.ToCharArray();

            //if((dsa[0] == 'C')&&(dsa[1] == 'M')) //this if may be removed if efficiency is an issue
            if(dsa.Length >= 6)
            {
                speed = dsa[5] - 48;
                m.setSpeed(speed);
                Debug.Print("speed set to " + speed);

                switch (dsa[3])
                {
                    case 'f':
                        //if (r > 8)
                            m.forwards();
                        //else m.break_now();
                        break;
                    case 's':
                        m.break_now();
                        break;
                    case 'b':
                        m.backwards();
                        break;
                    default:
                        break;
                }
                switch (dsa[4])
                {
                    case 'l':
                        m.turn_Left();
                        break;
                    case 'r':
                        m.turn_Right();
                        break;
                    case 's':
                        m.turn_Straight();
                        break;
                    default:
                        break;
                }
            }
        }

        public char [] byteToChar(byte [] inB)
        {
            /*
            char[] retVal = new char[inB.Length];

            for (int i = 0; i <= (inB.Length - 1); i++)
            {
                retVal[i] = (char)inB[i];
            }
            return retVal;
             * */
            return Encoding.UTF8.GetChars(inB);
        }
        
        /// <summary>
        /// runs in the background and halts the vehicle if necessary
        /// </summary>
        public void checker()
        {
            while (isCheckingAhead)
            {
                try
                {
                    r = Readouts.getIRReading();

                    if (m.isGoingForward)   //only do this if we are actually going forward
                    {
                        if (r < 8 || r > 66)
                            m.break_now(); //Vehicle should not go forward if ultrasonic sensor is too close or too far from a wall
                    }
                }
                catch
                {
                    Debug.Print("FAILED to check distance ahead, and stop if necessary");
                }
                Thread.Sleep(450);
            }
        }
    }
}
